
// import oimo.collision.geometry.*;
// import oimo.common.*;
// import oimo.dynamics.*;
// import oimo.dynamics.constraint.contact.*;
// import oimo.dynamics.constraint.joint.*;
// import oimo.m.Mat3;
// import oimo.m.Quat;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Shape } from "./Shape";
import { Vec3 } from "../../common/Vec3";
import { Transform } from "../../common/Transform";
import { Mat3 } from "../../common/Mat3";
import { M } from "../../common/M";
import { Setting } from "../../common/Setting";
import { MathUtil } from "../../common/MathUtil";
import { Quat } from "../../common/Quat";
import { RigidBodyConfig } from "./RigidBodyConfig";
import { RigidBodyType } from "./RigidBodyType";
import { ContactLink } from "../ContactLink";
import { JointLink } from "../constraint/joint/JointLink";
import { World } from "../World";
import { Geometry } from "../../collision/geometry/Geometry";
import { MassData } from "./MassData";

/**
 * A rigid body. To add a rigid body to a physics world, create a `RigidBody`
 * instance, create and add shapes via `RigidBody.addShape`, and add the rigid
 * body to the physics world through `World.addRigidBody`. Rigid bodies have
 * three motion types: dynamic, static, and kinematic. See `RigidBodyType` for
 * details of motion types.
 */
export class RigidBody {
	_next: RigidBody;
	_prev: RigidBody;

	_shapeList: Shape;
	_shapeListLast: Shape;
	_numShapes: number;

	_vel: Vec3;
	_angVel: Vec3;

	_pseudoVel: Vec3;
	_angPseudoVel: Vec3;

	_ptransform: Transform;
	_transform: Transform;

	_type: number;

	_sleepTime: number;
	_sleeping: boolean;
	_autoSleep: boolean;

	_mass: number;
	_invMass: number;
	_localInertia: Mat3;
	_rotFactor: Vec3;
	_invLocalInertia: Mat3;
	_invLocalInertiaWithoutRotFactor: Mat3;
	_invInertia: Mat3;

	_linearDamping: number;
	_angularDamping: number;

	_force: Vec3;
	_torque: Vec3;

	_world: World;

	_contactLinkList: ContactLink;
	_contactLinkListLast: ContactLink;
	_numContactLinks: number;

	_jointLinkList: JointLink;
	_jointLinkListLast: JointLink;
	_numJointLinks: number;

	_addedToIsland: boolean;
	_gravityScale: number;

	/**
	 * Extra field that users can use for their own purposes.
	 */
	userData: any;

	/**
	 * Creates a new rigid body by configuration `config`.
	 */
	constructor(config: RigidBodyConfig) {
		this._next = null;
		this._prev = null;

		this._shapeList = null;
		this._shapeListLast = null;
		this._numShapes = 0;

		this._contactLinkList = null;
		this._contactLinkListLast = null;
		this._numContactLinks = 0;

		this._jointLinkList = null;
		this._jointLinkListLast = null;
		this._numJointLinks = 0;

		this._vel = config.linearVelocity.clone();
		this._angVel = config.angularVelocity.clone();

		this._pseudoVel = new Vec3();
		this._angPseudoVel = new Vec3();

		this._ptransform = new Transform(config.position, config.rotation);
		this._transform = this._ptransform.clone();

		this._type = config.type;

		this._sleepTime = 0;
		this._sleeping = false;
		this._autoSleep = config.autoSleep;

		this._mass = 0;
		this._invMass = 0;
		this._localInertia = new Mat3();
		this._invLocalInertia = new Mat3();
		this._invLocalInertiaWithoutRotFactor = new Mat3();
		this._invInertia = new Mat3();

		this._linearDamping = config.linearDamping;
		this._angularDamping = config.angularDamping;

		this._force = new Vec3();
		this._torque = new Vec3();

		this._rotFactor = new Vec3(1, 1, 1);

		this._addedToIsland = false;
		this._gravityScale = 1;

		this._world = null;
	}

	// --- internal ---

	public _integrate(dt: number): void {
		switch (this._type) {
			case RigidBodyType.DYNAMIC:
			case RigidBodyType.KINEMATIC:
				var translation: Vec3 = new Vec3();
				var rotation: Vec3 = new Vec3();
				M.vec3_scale(translation, this._vel, dt);
				M.vec3_scale(rotation, this._angVel, dt);

				var translationLengthSq: number = M.vec3_dot(translation, translation);
				var rotationLengthSq: number = M.vec3_dot(rotation, rotation);

				if (translationLengthSq == 0 && rotationLengthSq == 0) {
					return; // no need of integration
				}

				// limit linear velocity
				if (translationLengthSq > Setting.maxTranslationPerStep * Setting.maxTranslationPerStep) {
					var l: number = Setting.maxTranslationPerStep / MathUtil.sqrt(translationLengthSq);
					M.vec3_scale(this._vel, this._vel, l);
					M.vec3_scale(translation, translation, l);
				}

				// limit angular velocity
				if (rotationLengthSq > Setting.maxRotationPerStep * Setting.maxRotationPerStep) {
					var l: number = Setting.maxRotationPerStep / MathUtil.sqrt(rotationLengthSq);
					M.vec3_scale(this._angVel, this._angVel, l);
					M.vec3_scale(rotation, rotation, l);
				}

				// update the transform
				(this._applyTranslation(translation));
				(this._applyRotation(rotation));
				break;
			case RigidBodyType.STATIC:
				this._vel.zero();
				this._angVel.zero();
				this._pseudoVel.zero();
				this._angPseudoVel.zero();
				break;
		}
	}

	public _integratePseudoVelocity(): void {
		var pseudoVelLengthSq: number = M.vec3_dot(this._pseudoVel, this._pseudoVel);
		var angPseudoVelLengthSq: number = M.vec3_dot(this._angPseudoVel, this._angPseudoVel);
		if (pseudoVelLengthSq == 0 && angPseudoVelLengthSq == 0) {
			return; // no need of intgration
		}

		switch (this._type) {
			case RigidBodyType.DYNAMIC:
			case RigidBodyType.KINEMATIC:
				var translation: Vec3 = new Vec3();
				var rotation: Vec3 = new Vec3();
				M.vec3_assign(translation, this._pseudoVel);
				M.vec3_assign(rotation, this._angPseudoVel);

				// clear pseudo velocity
				this._pseudoVel.zero();
				this._angPseudoVel.zero();

				// update the transform
				(this._applyTranslation(translation));
				(this._applyRotation(rotation));
				break;
			case RigidBodyType.STATIC:
				this._pseudoVel.zero();
				this._angPseudoVel.zero();
				break;
		}
	}


	public _isSleepy(): boolean {
		return this._autoSleep &&
			M.vec3_dot(this._vel, this._vel) < Setting.sleepingVelocityThreshold * Setting.sleepingVelocityThreshold &&
			M.vec3_dot(this._angVel, this._angVel) < Setting.sleepingAngularVelocityThreshold * Setting.sleepingAngularVelocityThreshold
			;
	}


	public _isAlone(): boolean {
		return this._numContactLinks == 0 && this._numJointLinks == 0;
	}


	public _applyTranslation(translation: Vec3): void {
		this._transform._position.addEq(translation);
	}


	public _applyRotation(rotation: Vec3): void {
		// compute derivative of the quaternion
		var theta: number = M.vec3_length(rotation);
		var halfTheta: number = theta * 0.5;
		var rotationToSinAxisFactor: number; // sin(halfTheta) / theta;
		var cosHalfTheta: number;            // cos(halfTheta)
		if (halfTheta < 0.5) {
			// use Maclaurin expansion
			var ht2: number = halfTheta * halfTheta;
			rotationToSinAxisFactor = (1 / 2) * (1 - ht2 * (1 / 6) + ht2 * ht2 * (1 / 120));
			cosHalfTheta = 1 - ht2 * (1 / 2) + ht2 * ht2 * (1 / 24);
		} else {
			rotationToSinAxisFactor = MathUtil.sin(halfTheta) / theta;
			cosHalfTheta = MathUtil.cos(halfTheta);
		}
		var sinAxis: Vec3 = new Vec3();
		M.vec3_scale(sinAxis, rotation, rotationToSinAxisFactor);
		var dq: Quat = new Quat();
		M.quat_fromVec3AndFloat(dq, sinAxis, cosHalfTheta);

		// integrate quaternion
		var q: Quat = new Quat();
		M.quat_fromMat3(q, this._transform._rotation);
		M.quat_mul(q, dq, q);
		q.normalize();

		// update rotation
		M.mat3_fromQuat(this._transform._rotation, q);

		// update inertia tensor
		this.updateInvInertia();
	}

	// call when added/removed/modified shapes

	public _shapeModified(): void {
		this.updateMass();
		this._syncShapes();
	}


	public _syncShapes(): void {
		var s: Shape = this._shapeList;
		while (s != null) {
			(s._sync(this._ptransform, this._transform));
			s = s._next;
		}
	}


	public _applyLinearPositionImpulse(imp: Vec3): void {
		var translation: Vec3 = new Vec3();
		M.vec3_scale(translation, imp, this._invMass);
		(this._applyTranslation(translation));
	}


	public _applyAngularPositionImpulse(imp: Vec3): void {
		var rotation: Vec3 = new Vec3();
		M.vec3_mulMat3(rotation, imp, this._invInertia);
		(this._applyRotation(rotation));
	}

	// --- ---

	public updateMass(): void {
		var totalInertia: Mat3 = new Mat3();
		var totalMass: number = 0;
		var inertia: Mat3 = new Mat3();

		var s: Shape = this._shapeList;
		while (s != null) {
			var g: Geometry = s._geom;
			g._updateMass();

			var mass: number = s._density * g._volume;

			// I_transformed = (R * I_localCoeff * R^T) * mass
			M.mat3_transformInertia(inertia, g._inertiaCoeff, s._localTransform._rotation);
			M.mat3_scale(inertia, inertia, mass);

			// I_cog = |  y*y+z*z  -x*y      -x*z     |
			//         | -x*y       x*x+z*z  -y*z     | * mass
			//         | -x*z      -y*z       x*x+y*y |
			// I = I_transformed + I_cog
			var cogInertia: Mat3 = new Mat3();
			M.mat3_inertiaFromCOG(cogInertia, s._localTransform._position);
			M.mat3_addRhsScaled(inertia, inertia, cogInertia, mass);

			// add mass data
			totalMass += mass;
			M.mat3_add(totalInertia, totalInertia, inertia);
			s = s._next;
		};

		this._mass = totalMass;
		this._localInertia.copyFrom(totalInertia);

		this.completeMassData();

		// wake up the rigid body
		this.wakeUp();
	}

	// compute inverse mass and inertias from _mass and _localInertia

	private completeMassData(): void {
		var det: number = M.mat3_det(this._localInertia);
		if (this._mass > 0 && det > 0 && this._type == RigidBodyType.DYNAMIC) {
			this._invMass = 1 / this._mass;
			M.mat3_inv(this._invLocalInertia, this._localInertia);
			M.mat3_assign(this._invLocalInertiaWithoutRotFactor, this._invLocalInertia);
			M.mat3_scaleRows(this._invLocalInertia, this._invLocalInertiaWithoutRotFactor, this._rotFactor.x, this._rotFactor.y, this._rotFactor.z);
		} else {
			// set mass and inertia zero
			this._invMass = 0;
			M.mat3_zero(this._invLocalInertia);
			M.mat3_zero(this._invLocalInertiaWithoutRotFactor);

			// force static
			if (this._type == RigidBodyType.DYNAMIC) {
				this._type = RigidBodyType.STATIC;
			}
		}
		this.updateInvInertia();
	}


	private updateInvInertia(): void {
		M.mat3_transformInertia(this._invInertia, this._invLocalInertia, this._transform._rotation);
		M.mat3_scaleRows(this._invInertia, this._invInertia, this._rotFactor.x, this._rotFactor.y, this._rotFactor.z);
	}

	// call when the transform is externally updated

	private updateTransformExt(): void {
		M.transform_assign(this._ptransform, this._transform);
		this._syncShapes();
		this.wakeUp();
	}

	// --- public ---

	/**
	 * Returns the world position of the rigid body.
	 */
	public getPosition(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._transform._position);
		return v;
	}

	/**
	 * Sets `position` to the world position of the rigid body.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getPositionTo(position: Vec3): void {
		M.vec3_toVec3(position, this._transform._position);
	}

	/**
	 * Sets the world position of the rigid body to `position`.
	 */
	public setPosition(position: Vec3): void {
		M.vec3_fromVec3(this._transform._position, position);
		this.updateTransformExt();
	}

	/**
	 * Translates the position of the rigid body by `translation`.
	 */
	public translate(translation: Vec3): void {
		var diff: Vec3 = new Vec3();
		M.vec3_fromVec3(diff, translation);
		M.vec3_add(this._transform._position, this._transform._position, diff);
		this.updateTransformExt();
	}

	/**
	 * Returns the rotation matrix of the rigid body.
	 */
	public getRotation(): Mat3 {
		var m: Mat3 = new Mat3();
		M.mat3_toMat3(m, this._transform._rotation);
		return m;
	}

	/**
	 * Sets `rotation` to the rotation matrix of the rigid body.
	 *
	 * This does not create a new instance of `Mat3`.
	 */
	public getRotationTo(rotation: Mat3): void {
		M.mat3_toMat3(rotation, this._transform._rotation);
	}

	/**
	 * Sets the rotation matrix of the rigid body to `rotation`.
	 */
	public setRotation(rotation: Mat3): void {
		M.mat3_fromMat3(this._transform._rotation, rotation);

		this.updateInvInertia();
		this.updateTransformExt();
	}

	/**
	 * Sets the rotation of the rigid body by Euler angles `eulerAngles` in radians.
	 */
	public setRotationXyz(eulerAngles: Vec3): void {
		var xyz: Vec3 = new Vec3();
		M.vec3_fromVec3(xyz, eulerAngles);
		M.mat3_fromEulerXyz(this._transform._rotation, xyz);

		this.updateInvInertia();
		this.updateTransformExt();
	}

	/**
	 * Rotates the rigid body by the rotation matrix `rotation`.
	 */
	public rotate(rotation: Mat3): void {
		var rot: Mat3 = new Mat3();
		M.mat3_fromMat3(rot, rotation);
		M.mat3_mul(this._transform._rotation, rot, this._transform._rotation);

		this.updateInvInertia();
		this.updateTransformExt();
	}

	/**
	 * Rotates the rigid body by Euler angles `eulerAngles` in radians.
	 */
	public rotateXyz(eulerAngles: Vec3): void {
		var xyz: Vec3 = new Vec3();
		var rot: Mat3 = new Mat3();
		M.vec3_fromVec3(xyz, eulerAngles);
		M.mat3_fromEulerXyz(rot, xyz);
		M.mat3_mul(this._transform._rotation, rot, this._transform._rotation);

		this.updateInvInertia();
		this.updateTransformExt();
	}

	/**
	 * Returns the rotation of the rigid body as a quaternion.
	 */
	public getOrientation(): Quat {
		var q: Quat = new Quat();
		var iq: Quat = new Quat();
		M.quat_fromMat3(iq, this._transform._rotation);
		M.quat_toQuat(q, iq);
		return q;
	}

	/**
	 * Sets `orientation` to the rotation quaternion of the rigid body.
	 *
	 * This does not create a new instance of `Quat`.
	 */
	public getOrientationTo(orientation: Quat): void {
		var iq: Quat = new Quat();
		M.quat_fromMat3(iq, this._transform._rotation);
		M.quat_toQuat(orientation, iq);
	}

	/**
	 * Sets the rotation of the rigid body from a quaternion `quaternion`.
	 */
	public setOrientation(quaternion: Quat): void {
		var q: Quat;
		M.quat_fromQuat(q, quaternion);
		M.mat3_fromQuat(this._transform._rotation, q);

		this.updateInvInertia();
		this.updateTransformExt();
	}

	/**
	 * Returns the transform of the rigid body.
	 */
	public getTransform(): Transform {
		return this._transform.clone();
	}

	/**
	 * Sets `transform` to the transform of the rigid body.
	 *
	 * This does not create a new instance of `Transform`.
	 */
	public getTransformTo(transform: Transform): void {
		transform.copyFrom(this._transform);
	}

	/**
	 * Sets the transform of the rigid body to `transform`.
	 *
	 * This does not keep any references to `transform`.
	 */
	public setTransform(transform: Transform): void {
		M.vec3_assign(this._transform._position, transform._position);
		M.mat3_assign(this._transform._rotation, transform._rotation);

		this.updateInvInertia();
		this.updateTransformExt();
	}

	/**
	 * Returns the mass of the rigid body.
	 *
	 * If the rigid body has infinite mass, `0` will be returned.
	 */
	public getMass(): number {
		return this._mass;
	}

	/**
	 * Returns the moment of inertia tensor in local space.
	 */
	public getLocalInertia(): Mat3 {
		var m: Mat3 = new Mat3();
		M.mat3_toMat3(m, this._localInertia);
		return m;
	}

	/**
	 * Sets `inertia` to the moment of inertia tensor in local space.
	 *
	 * This does not create a new instance of `Mat3`
	 */
	public getLocalInertiaTo(inertia: Mat3): void {
		M.mat3_toMat3(inertia, this._localInertia);
	}

	/**
	 * Returns the mass data of the rigid body.
	 */
	public getMassData(): MassData {
		var md: MassData = new MassData();
		md.mass = this._mass;
		M.mat3_toMat3(md.localInertia, this._localInertia);
		return md;
	}

	/**
	 * Sets `massData` to the mass data of the rigid body.
	 *
	 * This does not create a new instance of `MassData`.
	 */
	public getMassDataTo(massData: MassData): void {
		massData.mass = this._mass;
		M.mat3_toMat3(massData.localInertia, this._localInertia);
	}

	/**
	 * Sets the mass and moment of inertia of the rigid body by the mass data `massData`.
	 * The properties set by this will be overwritten when
	 *
	 * - some shapes are added or removed
	 * - the type of the rigid body is changed
	 */
	public setMassData(massData: MassData): void {
		this._mass = massData.mass;
		M.mat3_fromMat3(this._localInertia, massData.localInertia);
		this.completeMassData();
		this.wakeUp();
	}

	/**
	 * Returns the rotation factor of the rigid body.
	 */
	public getRotationFactor(): Vec3 {
		return this._rotFactor.clone();
	}

	/**
	 * Sets the rotation factor of the rigid body to `rotationFactor`.
	 *
	 * This changes moment of inertia internally, so that the change of
	 * angular velocity in **global space** along X, Y and Z axis will scale by `rotationFactor.x`,
	 * `rotationFactor.y` and `rotationFactor.z` times respectively.
	 */
	public setRotationFactor(rotationFactor: Vec3): void {
		this._rotFactor.copyFrom(rotationFactor);

		this.updateInvInertia();
		this.wakeUp();
	}

	/**
	 * Returns the linear velocity of the rigid body.
	 */
	public getLinearVelocity(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._vel);
		return v;
	}

	/**
	 * Sets `linearVelocity` to the linear velocity of the rigid body.
	 *
	 * This does not create a new intrance of `Vec3`.
	 */
	public getLinearVelocityTo(linearVelocity: Vec3): void {
		M.vec3_toVec3(linearVelocity, this._vel);
	}

	/**
	 * Sets the linear velocity of the rigid body.
	 */
	public setLinearVelocity(linearVelocity: Vec3): void {
		if (this._type == RigidBodyType.STATIC) {
			M.vec3_zero(this._vel);
		} else {
			M.vec3_fromVec3(this._vel, linearVelocity);
		}
		this.wakeUp();
	}

	/**
	 * Returns the angular velocity of the rigid body.
	 */
	public getAngularVelocity(): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_toVec3(v, this._angVel);
		return v;
	}

	/**
	 * Sets `angularVelocity` to the angular velocity of the rigid body.
	 *
	 * This does not create a new intrance of `Vec3`.
	 */
	public getAngularVelocityTo(angularVelocity: Vec3): void {
		M.vec3_toVec3(angularVelocity, this._vel);
	}

	/**
	 * Sets the angular velocity of the rigid body.
	 */
	public setAngularVelocity(angularVelocity: Vec3): void {
		if (this._type == RigidBodyType.STATIC) {
			M.vec3_zero(this._angVel);
		} else {
			M.vec3_fromVec3(this._angVel, angularVelocity);
		}
		this.wakeUp();
	}

	/**
	 * Adds `linearVelocityChange` to the linear velcity of the rigid body.
	 */
	public addLinearVelocity(linearVelocityChange: Vec3): void {
		if (this._type != RigidBodyType.STATIC) {
			var d: Vec3 = new Vec3();
			M.vec3_fromVec3(d, linearVelocityChange);
			M.vec3_add(this._vel, this._vel, d);
		}
		this.wakeUp();
	}

	/**
	 * Adds `angularVelocityChange` to the angular velcity of the rigid body.
	 */
	public addAngularVelocity(angularVelocityChange: Vec3): void {
		if (this._type != RigidBodyType.STATIC) {
			var d: Vec3 = new Vec3();
			M.vec3_fromVec3(d, angularVelocityChange);
			M.vec3_add(this._angVel, this._angVel, d);
		}
		this.wakeUp();
	}

	/**
	 * Applies the impulse `impulse` to the rigid body at `positionInWorld` in world position.
	 *
	 * This changes both the linear velocity and the angular velocity.
	 */
	public applyImpulse(impulse: Vec3, positionInWorld: Vec3): void {
		// linear
		var imp: Vec3 = new Vec3();
		M.vec3_fromVec3(imp, impulse);
		M.vec3_addRhsScaledEQ(this._vel, this._vel, imp, this._invMass);

		// angular
		var aimp: Vec3 = new Vec3();
		var pos: Vec3 = new Vec3();
		M.vec3_fromVec3(pos, positionInWorld);
		M.vec3_sub(pos, pos, this._transform._position);
		M.vec3_cross(aimp, pos, imp);
		M.vec3_mulMat3(aimp, aimp, this._invInertia);
		M.vec3_add(this._angVel, this._angVel, aimp);

		this.wakeUp();
	}

	/**
	 * Applies the linear impulse `impulse` to the rigid body.
	 *
	 * This does not change the angular velocity.
	 */
	public applyLinearImpulse(impulse: Vec3): void {
		var imp: Vec3 = new Vec3();
		M.vec3_fromVec3(imp, impulse);
		M.vec3_addRhsScaledEQ(this._vel, this._vel, imp, this._invMass);
		this.wakeUp();
	}

	/**
	 * Applies the angular impulse `impulse` to the rigid body.
	 *
	 * This does not change the linear velocity.
	 */
	public applyAngularImpulse(impulse: Vec3): void {
		var imp: Vec3 = new Vec3();
		M.vec3_fromVec3(imp, impulse);
		M.vec3_mulMat3(imp, imp, this._invInertia);
		M.vec3_add(this._angVel, this._angVel, imp);
		this.wakeUp();
	}

	/**
	 * Applies the force `force` to `positionInWorld` in world position.
	 */
	public applyForce(force: Vec3, positionInWorld: Vec3): void {
		// linear
		var iforce: Vec3 = new Vec3();
		M.vec3_fromVec3(iforce, force);
		M.vec3_add(this._force, this._force, iforce);

		// angular
		var itorque: Vec3 = new Vec3();
		var pos: Vec3 = new Vec3();
		M.vec3_fromVec3(pos, positionInWorld);
		M.vec3_sub(pos, pos, this._transform._position);
		M.vec3_cross(itorque, pos, iforce);
		M.vec3_add(this._torque, this._torque, itorque);

		this.wakeUp();
	}

	/**
	 * Applies the force `force` to the center of mass.
	 */
	public applyForceToCenter(force: Vec3): void {
		// linear
		var iforce: Vec3 = new Vec3();
		M.vec3_fromVec3(iforce, force);
		M.vec3_add(this._force, this._force, iforce);

		this.wakeUp();
	}

	/**
	 * Applies the torque `torque`.
	 */
	public applyTorque(torque: Vec3): void {
		// angular
		var itorque: Vec3 = new Vec3();
		M.vec3_fromVec3(itorque, torque);
		M.vec3_add(this._torque, this._torque, itorque);

		this.wakeUp();
	}

	/**
	 * Returns the gravity scaling factor of the rigid body.
	 */
	public getGravityScale(): number {
		return this._gravityScale;
	}

	/**
	 * Sets the gravity scaling factor of the rigid body to `gravityScale`.
	 *
	 * If `0` is set, the rigid body will not be affected by gravity.
	 */
	public setGravityScale(gravityScale: number): void {
		this._gravityScale = gravityScale;
		this.wakeUp();
	}

	/**
	 * Returns the local coordinates of the point `worldPoint` in world coodinates.
	 */
	public getLocalPoint(worldPoint: Vec3): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, worldPoint);
		M.vec3_sub(v, v, this._transform._position);
		M.vec3_mulMat3Transposed(v, v, this._transform._rotation);
		var res: Vec3 = new Vec3();
		M.vec3_toVec3(res, v);
		return res;
	}

	/**
	 * Sets `localPoint` to the local coordinates of the point `worldPoint` in world coodinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalPointTo(worldPoint: Vec3, localPoint: Vec3): void {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, worldPoint);
		M.vec3_sub(v, v, this._transform._position);
		M.vec3_mulMat3Transposed(v, v, this._transform._rotation);
		M.vec3_toVec3(localPoint, v);
	}

	/**
	 * Returns the local coordinates of the vector `worldVector` in world coodinates.
	 */
	public getLocalVector(worldVector: Vec3): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, worldVector);
		M.vec3_mulMat3Transposed(v, v, this._transform._rotation);
		var res: Vec3 = new Vec3();
		M.vec3_toVec3(res, v);
		return res;
	}

	/**
	 * Sets `localVector` to the local coordinates of the vector `worldVector` in world coodinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getLocalVectorTo(worldVector: Vec3, localVector: Vec3): void {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, worldVector);
		M.vec3_mulMat3Transposed(v, v, this._transform._rotation);
		M.vec3_toVec3(localVector, v);
	}

	/**
	 * Returns the world coordinates of the point `localPoint` in local coodinates.
	 */
	public getWorldPoint(localPoint: Vec3): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, localPoint);
		M.vec3_mulMat3(v, v, this._transform._rotation);
		M.vec3_add(v, v, this._transform._position);
		var res: Vec3 = new Vec3();
		M.vec3_toVec3(res, v);
		return res;
	}

	/**
	 * Sets `worldPoint` to the world coordinates of the point `localPoint` in local coodinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getWorldPointTo(localPoint: Vec3, worldPoint: Vec3): void {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, localPoint);
		M.vec3_mulMat3(v, v, this._transform._rotation);
		M.vec3_add(v, v, this._transform._position);
		M.vec3_toVec3(worldPoint, v);
	}

	/**
	 * Returns the world coordinates of the vector `localVector` in local coodinates.
	 */
	public getWorldVector(localVector: Vec3): Vec3 {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, localVector);
		M.vec3_mulMat3(v, v, this._transform._rotation);
		var res: Vec3 = new Vec3();
		M.vec3_toVec3(res, v);
		return res;
	}

	/**
	 * Sets `worldVector` to the world coordinates of the vector `localVector` in local coodinates.
	 *
	 * This does not create a new instance of `Vec3`.
	 */
	public getWorldVectorTo(localVector: Vec3, worldVector: Vec3): void {
		var v: Vec3 = new Vec3();
		M.vec3_fromVec3(v, localVector);
		M.vec3_mulMat3(v, v, this._transform._rotation);
		M.vec3_toVec3(worldVector, v);
	}

	/**
	 * Returns the number of the shapes added.
	 */
	public getNumShapes(): number {
		return this._numShapes;
	}

	/**
	 * Returns the list of the shapes of the rigid body.
	 */
	public getShapeList(): Shape {
		return this._shapeList;
	}

	/**
	 * Returns the number of the contact lists the rigid body is involved.
	 */
	public getNumContectLinks(): number {
		return this._numContactLinks;
	}

	/**
	 * Returns the list of the contact links the rigid body is involved.
	 */
	public getContactLinkList(): ContactLink {
		return this._contactLinkList;
	}

	/**
	 * Returns the number of the joint links the rigid body is attached.
	 */
	public getNumJointLinks(): number {
		return this._numJointLinks;
	}

	/**
	 * Returns the list of the joint links the rigid body is attached.
	 */
	public getJointLinkList(): JointLink {
		return this._jointLinkList;
	}

	/**
	 * Adds the shape to the rigid body.
	 */
	public addShape(shape: Shape): void {
		// first, add the shape to the linked list so that it will be considered
		if (this._shapeList == null) {
			this._shapeList = shape;
			this._shapeListLast = shape;
		} else {
			this._shapeListLast._next = shape;
			shape._prev = this._shapeListLast;
			this._shapeListLast = shape;
		}
		this._numShapes++;
		shape._rigidBody = this;

		// then add the shape to the world
		if (this._world != null) {
			this._world._addShape(shape);
		}

		this._shapeModified();
	}

	/**
	 * Removes the shape from the rigid body.
	 */
	public removeShape(shape: Shape): void {
		// first, remove the shape from the linked list so that it will be ignored

		var prev = shape._prev;
		var next = shape._next;
		if (prev != null) {
			prev._next = next;
		}
		if (next != null) {
			next._prev = prev;
		}
		if (shape == this._shapeList) {
			this._shapeList = next;
		}
		if (shape == this._shapeListLast) {
			this._shapeListLast = prev;
		}
		shape._prev = null;
		shape._next = null;

		this._numShapes--;
		shape._rigidBody = null;

		// then remove the shape from the world
		if (this._world != null) {
			this._world._removeShape(shape);
		}

		this._shapeModified();
	}

	/**
	 * Returns the rigid body's type of behaviour.
	 *
	 * See `RigidBodyType` class for details.
	 */
	public getType(): number {
		return this._type;
	}

	/**
	 * Sets the rigid body's type of behaviour.
	 *
	 * See `RigidBodyType` class for details.
	 */
	public setType(type: number): void {
		this._type = type;
		this.updateMass();
	}

	/**
	 * Sets the rigid body's sleep flag false.
	 *
	 * This also resets the sleeping timer of the rigid body.
	 */
	public wakeUp(): void {
		this._sleeping = false;
		this._sleepTime = 0;
	}

	/**
	 * Sets the rigid body's sleep flag true.
	 *
	 * This also resets the sleeping timer of the rigid body.
	 */
	public sleep(): void {
		this._sleeping = true;
		this._sleepTime = 0;
	}

	/**
	 * Returns whether the rigid body is sleeping.
	 */
	public isSleeping(): boolean {
		return this._sleeping;
	}

	/**
	 * Returns how long the rigid body is stopping moving. This returns `0` if the body
	 * has already slept.
	 */
	public getSleepTime(): number {
		return this._sleepTime;
	}

	/**
	 * Sets the rigid body's auto sleep flag.
	 *
	 * If auto sleep is enabled, the rigid body will automatically sleep when needed.
	 */
	public setAutoSleep(autoSleepEnabled: boolean): void {
		this._autoSleep = autoSleepEnabled;
		this.wakeUp();
	}

	/**
	 * Returns the linear damping.
	 */
	public getLinearDamping(): number {
		return this._linearDamping;
	}

	/**
	 * Sets the linear damping to `damping`.
	 */
	public setLinearDamping(damping: number): void {
		this._linearDamping = damping;
	}

	/**
	 * Returns the angular damping.
	 */
	public getAngularDamping(): number {
		return this._angularDamping;
	}

	/**
	 * Sets the angular damping to `damping`.
	 */
	public setAngularDamping(damping: number): void {
		this._angularDamping = damping;
	}

	/**
	 * Returns the previous rigid body in the world.
	 *
	 * If the previous one does not exist, `null` will be returned.
	 */
	public getPrev(): RigidBody {
		return this._prev;
	}

	/**
	 * Returns the next rigid body in the world.
	 *
	 * If the next one does not exist, `null` will be returned.
	 */
	public getNext(): RigidBody {
		return this._next;
	}

}
